#include <btl/Utility/Converters.hpp>
using namespace btl::utility;

#include <iostream>
#include <vector>
#include <algorithm>


vector< vector< int > >  combination( const vector< int>& vN_, const int nM_ )
{
//get M numbers from the set vN_
//vN_ contains the data you wanna combine, for example vN_ = [ 1, 2, 3 ]
//the results will be save in a vector of vector, for example the 2 out of [ 1, 2, 3 ] and it will be:
// [ [ 1, 2 ] [ 2, 3 ] ]
//
	CHECK( vN_.size() >= nM_, "you cant extract 6 numbers out of 5." );
	vector< vector< int > > vvResult;
	if( 1 == nM_ )
	{
	// when get 1 data out of n, simply has n possibilities
		for( vector< int >::const_iterator cit = vN_.begin(); cit!=vN_.end(); cit++ )
		{
			vector< int > vNumbers;
			vNumbers.push_back( *cit );
			vvResult.push_back( vNumbers );
		}
	}
	else
	{
		for( int i = 0; i < vN_.size() - (nM_ - 1) ; i++ )
		{
			int nData = vN_[i] ; 
			//colect the rest of data from vN_ into vNM1 ( vN - 1 );
			//for example vN_ = [ 1, 2, 3, 4, 5 ]
			//          and i = 2
			//           vNM1 = [ 4, 5 ]  
			vector< int > vNM1;
			for( int j = i+1; j < vN_.size(); j++ )
			{
				vNM1.push_back( vN_[j] );
			}
			//get the combinations of vNM1 and nM_-1;
			vector< vector< int > > vvRStep = combination( vNM1, nM_-1 );
			//insert the nData to each combination and insert all combinations into the overall results 
			for( vector< vector< int > >::iterator it_vv = vvRStep.begin(); it_vv != vvRStep.end(); it_vv++ )
			{
				it_vv->push_back( nData ); 
				vvResult.push_back( *it_vv );
			}
		}
	}
	return vvResult;
}

int main()
{
	// create a data vector containing the datas you wanna to combine.
	vector< int > vData;
	for( int i = 1; i<= 6; i++)
	{
		vData.push_back( i );
	}
	// main combination functions
	vector< vector < int > > vvResult = combination( vData, 3 );
	// output
	PRINT( vData );
	PRINT( vvResult );
	return 0;
}

/*
// test pointer
#include <btl/Utility/Converters.hpp>
#include <stdio.h>

void assign( double* pPointer_, int nN_ )
{
	for( int i=0;i<nN_;i++)
	{
		*pPointer_++ = i;
	}
}

int main()
{
	double* pXYZ = new double[ 10 ];
	double* pMoving = pXYZ;

	assign( pXYZ, 10 );
	for( int i=0;i<10;i++)
	{
		PRINT( *pMoving++ );
	}
	pMoving = pXYZ;
	memset( pMoving, 0, 10*sizeof(double)); 
	for( int i=0;i<10;i++)
	{
		PRINT( *pMoving++ );
	}
	pMoving = new double[ 10 ];
	memcpy( pMoving, pXYZ, 10*sizeof(double) );
	double* pM = pMoving;
	for( int i=0;i<10;i++)
	{
		PRINT( *pM++ );
	}

	delete [] pXYZ;
	delete [] pMoving;
}
*/
/*
// test the function absoluteOrientation()
#include <btl/Utility/Converters.hpp>
using namespace std;
using namespace cv;
using namespace btl::utility;

int main()
{
	//constructing the testsuite
	
	Eigen::Vector3d eivT;
	eivT << 45, 
			-78, 
			98;
	Eigen::Matrix3d eimR;
	eimR << 0.36, 0.48, -0.8,
			-0.8, 0.6,  0,
			0.48, 0.64, 0.6;
	Eigen::MatrixXd eimX(3,4), eimY(3,4), Noise(3,4);

	eimX << 0.272132, 0.538001, 0.755920, 0.582317,
            0.728957, 0.089360, 0.507490, 0.100513,
            0.578818, 0.779569, 0.136677, 0.785203;
	
	Noise << -0.23, -0.01, 0.03, -0.06,
			  0.07, -0.09,-0.037, -0.08,
			  0.009, 0.09, -0.056, 0.012;
	double dS = 1.0;
	for( int i=0; i< eimX.cols(); i++ )
	{
		Eigen::Vector3d eivPt;
		eimY.col(i) = dS*eimR*eimX.col(i) + eivT;
	}
	//eimY += Noise;
    Eigen::Matrix3d eimR2;
	Eigen::Vector3d eivT2;
	double dS2;
 	double dError = absoluteOrientation< double >( eimX, eimY, false, &eimR2, &eivT2, &dS2 );
	PRINT( dError );
	PRINT( eimR2 );
	PRINT( eimR );
	PRINT( eivT2 );
	PRINT( eivT );
	PRINT( dS2 );
	PRINT( dS );

	return 0;
}
*/

/*
// test optim.hpp and optim.cpp
#include <btl/Utility/Converters.hpp>
#include "optim.hpp"
int main()
{
	cout << "start ";
    shuda::COptim o;
		try{
	o.SetMdAlg(shuda::COptim::CONJUGATE);
	o.Go();
	o.SetMdAlg(shuda::COptim::DIRECTIONSETS);
	o.Go();
	o.SetMdAlg(shuda::COptim::GRADIENTDESCENDENT);
	o.Go();

	}
	catch ( CError& e )
    {
        if ( string const* mi = boost::get_error_info< CErrorInfo > ( e ) )
        {
            std::cerr << "Error Info: " << *mi << std::endl;
        }
    }

	return 0;
}
*/
/*
// test the function to convert kinect raw depth to meter
#include <opencv/highgui.h>
#include <opencv/cv.h>
#include <string>
#include <btl/Utility/Converters.hpp>

using namespace std;
using namespace cv;
using namespace btl::utility;

int main()
{
    Mat cvmDepth = cv::imread ( "depthUndistorted0.bmp" );
	Mat_<double> mDK= ( Mat_<double>(3, 1) << 1.16600151976717, 2842.513906334149, 0.1205634721557855 );
	PRINT( mDK );
	double dDepth = depthInMeters<double> ( 545, 156, cvmDepth, mDK, 1 );
    PRINT( dDepth );
	Mat_<char> cvmColor = getColor< char > ( 545, 156, cvmDepth );
	//cvmColor /= 255.;
    PRINT( cvmColor );
    return 0;
}
*/
/*
//test << converting from Mat_<double> to CvMat
//test << converting from CvMat to Mat_<double>
#include <btl/Utility/Converters.hpp>
#include <opencv/highgui.h>
#include <opencv/cv.h>
#include <string>
using namespace std;
using namespace cv;
using namespace btl::utility;
int main()
{
	try{

	Mat_<double> cppK; cppK = ( Mat_<double>(3,3) << 1,2,3,4,5,6,7,8,9 );
	PRINT( cppK );

  	CvMat* pcK = cvCreateMat( cppK.rows, cppK.cols, CV_64F );

	*pcK << cppK;
	Mat_<double> cppK2;
	cppK2 << *pcK;

    //assignPtr( &cppK, pcK );
	//assignPtr( pcK, &cppK );

	PRINT( cppK2 );

	}
	catch ( CError& e )
    {
        if ( string const* mi = boost::get_error_info< CErrorInfo > ( e ) )
        {
            std::cerr << "Error Info: " << *mi << std::endl;
        }
    }
	return 0;
}
*/

